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Whole Body Operational Space Control (WBOSC) is a pioneering algorithm in the 
field of human-centered Whole-Body Control (WBC). It enables floating-base highly- 
redundant robots to achieve unified motion/force control of one or more operational 
space objectives while adhering to physical constraints. Although there are extensive 
studies on the algorithms and theory behind WBOSC, limited studies exist on the soft¬ 
ware architecture and APIs that enable WBOSC to perform and be integrated into 
a larger system. In this paper we address this by presenting Controllt!, a new open- 
source software framework for WBOSC. Unlike previous implementations, Controllt! is 
multi-threaded to increase servo frequencies on standard PC hardware. A new parameter 
binding mechanism enables tight integration between Controllt! and external processes 
via an extensible set of transport protocols. To support a new robot, only two plugins 
and a URDF model needs to be provided — the rest of Controllt! remains unchanged. 
New WBC primitives can be added by writing a Task or Constraint plugin. Controlltl’s 
capabilities are demonstrated on Dreamer, a 16-DOF torque controlled humanoid upper 
body robot containing both series elastic and co-actuated joints, and using it to perform 
a product disassembly task. Using this testbed, we show that Controllt! can achieve av¬ 
erage servo latencies of about 0.5ms when configured with two Cartesian position tasks, 
two orientation tasks, and a lower priority posture task. This is significantly higher than 
the 5ms that was achieved using UTA-WBC, the prototype implementation of WBOSC 
that is both application and platform-specific. Variations in the product’s position is 
handled by updating the goal of the Cartesian position task. Controlltl’s source code is 
released under an LGPL license and we hope it will be adopted and maintained by the 
WBC community for the long term as a platform for WBC development and integration. 

Keywords: Software Framework; Whole Body Control; Whole Body Operational Space 
Control; Upperbody Humanoid Robot 


1. Introduction 

Whole Body Control (WBC) takes a holistic view of a multi-branched highly re¬ 
dundant robot like humanoids to achieve general coordinated behaviors. One of the 
first WBC algorithms is Whole Body Operational Space Control (WBOSC) [ 310 , 
which provides the theoretical foundations for achieving operational space inverse 
dynamics, task prioritization, free floating degrees of freedom, contact constraints, 
and internal forces. There is now a growing community of researchers in this field 
as exemplified by the recent formation of an IEEE technical committee on WBC® 
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While the foundational theory and algorithms behind WBC have recently made 
great strides, less progress exists in software support, limiting the use of WBC 
today. In this paper, we remedy this problem by presenting Controlltlj^ an open 
sourc^E software framework for WBOSC. 

In this paper, we introduce Controllt!, a software framework that en¬ 
ables WBOSC controllers to be instantiated and is designed for systems 
integration, extensibility, high performance, and use by both WBC re¬ 
searchers and the general public. Instantiating a WBOSC controller consists 
of defining a prioritized compound task that defines the operational space objec¬ 
tives and lower priority goal postures that the controller should achieve, and a 
constraint set that specifies the natural physical constraints of the robot. Systems 
integration is achieved through a parameter binding mechanism that enables exter¬ 
nal processes to access WBOSC parameters through various transport layers, and 
a set of introspection tools for gaining insight into the controller’s state at run¬ 
time. Controllt! is extensible through the use of plugins that enable the addition 
of new WBC primitives and support for new robot platforms. High performance 
is achieved by using state-of-the-art software libraries and multiple-threads that 
enable Controllt! to offer higher servo frequencies relative to previous WBOSC im¬ 
plementations. By making Controllt! open source and maintaining a centralized 
website (https;//robotcontrolit. com) with detailed documentation, installation 
instructions, and tutorials, Controllt! can be modified to evaluate new WBC ideas 
and supported long term. 

The intellectual merit and key contributions of this paper are as follows: 

(1) We design a software architecture for supporting general use of WBOSC and 
its integration within a larger system via parameter binding and events. 

(2) We introduce the first API based on WBOSC principles for use across general 
applications and robots. 

(3) We provide an open-source software implementation. 

(4) We design and implement a high performance multi-threaded architecture that 
increases the achievable servo frequency by lOX relative to previous implemen¬ 
tations of WBOSC. 

(5) We reduce the number of components that need to be modified to develop 
a new behavior to the set of Robotinterface, ServoClock, CompoundTask, 
ConstraintSet and decouple these changes from core Controllt! code via dy¬ 
namically loadable plugins. 


“Controllt! should not be associated with Movelt! El. Controllt! is primarily focused on whole 
body feedback control whereas Movelt! is primarily focused on motion planning. Thus, Movelt! 
and Controllt! typically reside at different levels of a robot application’s software stack. The default 
feedback controller used by Movelt! is ros_control El. However, Movelt! could be configured to 
work with Controllt! instead of ros_control if needed. 

'’The source code for Controllt! is available under a LGPLv2.1 license. Instructions for downloading 
and using it are available at https://robotcontrolit.com 
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(6) We demonstrate Controlltl’s utility and performance using a humanoid robot 

executing a product disassembly task. 

The remainder of this paper is organized as follows. Section discusses related 
work. Section provides an overview of WBOSC’s mathematical foundations. Sec¬ 
tion presents Controlltl’s software architecture and APIs. Sectionpresents how 
Controllt! was integrated with Dreamer and used to develop a product disassem¬ 
bly task. Sectioncontains a discussion on other experiences using Controllt! and 
future research directions. The paper ends with conclusions in Section 

2. Related Work 

As a field, WBC is rapidly evolving. Most algorithms issue torque com¬ 
mands ri^l » | iu | ii | i^ | i^ | i4 | i5 | ib | i7 | i8 | m | :iupi | 22 | 23 | ^4p5 ^ differ in whether they 

are centralized [11I2ZI or distributed IHIlEl, focus on manipulation loco¬ 
motion | 31 | 32 | 33 |^ qj. behavior sequencing [Mill, the underlying control models 
used | 36 | 37 | 38 |^ whether they’ve been evaluated in simulation or on hard- 

|39|40|41|42|43|44|45|46|47|48|49|50|51|52|53|54|55|56|57|58|59|60|61|62|63|64|65|66|67| These 

efforts demonstrate the behaviors enabled by WBC such as the use of compliance, 
multi-contact postures, robot dynamics, and joint redundancy to balance multiple 
competing objectives. Controllt! is currently focused on supporting general use of 
WBOSC and its capabilities, but may be enhanced to include ideas and capabilities 
from these recent WBC developments. 

An implementation of WBOSC called Stanford-WBC was released in 2011. 
Stanford-WBC includes mechanisms for parameter reflection, data logging, and 
script-based configuration, but was a limited implementation of WBOSC that did 
not support branched robots, mobile robots, or contact constraints. It was used 
to make Dreamer’s right arm wave and shake hands. More recently, UTA-WBC 
extended Stanford-WBC to support the full WBOSC algorithm, which includes 
branched robots, free floating degrees of freedom, contact constraints, and a more 
accurate robot model that includes rotor inertias UTA-WBC was used to make 
a wheeled version of Dreamer containing 13 DOFs maintain balance on rough ter¬ 
rain. While this demonstrated the feasibility of WBOSC using a real humanoid 
robot, UTA-WBC was a research prototype targeted for a specific robot and spe¬ 
cific behavior, i.e., balancing [^. The implementation was not designed to work as 
part of a larger system for general applications. Instead, Controllt! is a complete 
software re-design and re-implementation of the WBOSC algorithm with a focus 
on the software constructs and APIs that facilitate the integration of WBOSC into 
larger systems. 

The differences between UTA-WBC and Controllt! are shown in Table [TJ 
Compared with UTA-WBC and Stanford-WBC, Controllt! is a complete re¬ 
implementation that does not build upon but rather replaces the previous im¬ 
plementation. Specifically, Controllt! contains new and more expressive software 
abstractions that enable arbitrarily complex WBOSC controllers to be configured. 
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Property 

UTA-WBC 

Controllt! 

OS 

Ubuntu 10.04 

Ubuntu 12.04 and 14.04 

ROS Integration 

ROS Fuerte 

ROS Hydro and Indigo 

Linear Algebra Library 

Eigen 2 

Eigen 3 

Model Library 

Tao 

RBDL 2.3.2 

Model Description Format 

Proprietary XML 

URDF 

Integration (higher levels) 

N/A 

Parameter binding 

Integration (lower levels) 

Proprietary 

Robotinterface and 
ServoClock plugins 

Controller Introspection 

Parameter Reflection 

Parameter Reflection and 

ROS Services 

WBC Initial Configuration 

YAML 

YAML and ROS parameter 

server 

WBC Reconfiguration 

N/A 

Enable / disable tasks and 
constraints, update task 
priority levels 

Key Abstractions 

task, constraint, skill 

Compound task, constraint 
set 

Task / Constraint Libraries 

Statically coded 

Dynamically loadable via 

ROS pluginlib 

Number of threads 

1 

3 

Simulator 

Proprietary 

Gazebo 5.1 

Website 

https://github.com/ 

https: 


Isentis/ 

//robotcontrolit.com 


uta-wbc-dreamer 



Table 1. A comparison between UTA-WBC and Controllt! 


works with newer software libraries, middleware, and simulators, supports exten¬ 
sibility through a plugin-based architecture, is multi-threaded, and is designed to 
easily integrate with external processes through parameter binding and controller 
introspection mechanisms. 

The ability to integrate with external processes is important because applica¬ 
tions of branched highly-redundant robots of the type targeted by WBC are typ¬ 
ically very sophisticated involving many layers of software both above and below 
the whole body controller. To handle such complexity, a distributed component- 
based software architecture is typically used where the application consists of 
numerous independently-running software processes or threads that communicate 
over both synchronous and asynchronous channels ll2lZH The importance of dis¬ 
tributed component-based software for advanced robotics is illustrated by the num- 
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ber of recently developed middleware frameworks that provide it. They include 
OpenHRP ElIZH RT-Middleware Orocos Toolchain YARP ROS EZEH, 
CLARAtyEHol, aRPI^, Microblx Him, OpenRDK HMUlll, and ERSPl^. Among 
these, Controllt! is currently integrated with ROS and is a ROS node within a 
ROS network, though usually as a real-time process potentially within another 
component-based framework (i.e., Controllt !’s servo thread was an Orocos real-time 
task during the DRC Trials, and is a RTAI real-time process in the Dreamer ex¬ 
periments discussed in this paper). In general, Controllt! can be modified to be a 
component within any of the other aforementioned component-based robot middle¬ 
ware frameworks. 

Controllt! is designed to interact with components both below (i.e., closer to the 
hardware) and above (i.e., closer to the end user or application) it within a robotic 
system. Components below Controllt! include robot hardware drivers or resource 
allocators like ros_control EMI and Conmanl^that manage how a robot’s joints 
are distributed among multiple controllers within the system. This is necessary since 
multiple WBC controllers may coexist and a manager is needed to ensure only one 
is active at a time. In addition, joints in a robots’ extremity like those in an end 
effector usually have separate dedicated controllers. Components that may reside 
above Controllt! include task specification frameworks like iTaSC EIMMIlll, plan¬ 
ners like Movelt! 1^, management tools like Rock 1^, MARCO 1^, and G®''oM 1^, 
behavior sequencing frameworks like Ecto^and RTC^^, and other frameworks for 
achieving machine autonomy Clearly, the set of compo¬ 

nents that Controllt! interacts with is large, dynamic, and application-dependent. 
This is possible since component-based architectures provide sufficient decoupling 
to allow these external components to change without requiring Controllt! to be 
modified. 

3. Overview of Whole Body Operational Space Control 

This section provides a brief overview of WBOSC. Details are provided in previous 
publications EEEEZl Let rijoints be the number of actual DOEs in the robot. The 
robot’s joint state is represented by the vector qactuai as shown by the following 
equation. 

Qactuai —^ Ql - - • QrijointB ^ (^) 

The robot’s global pose is represented by a 6-dimensional floating virtual joint 
that connects the robot’s base link to the world, i.e., three rotational and three 
prismatic virtual joints. It is denoted by vector Qbase € R®- The two partial state 
vectors, Qactuai and Qbase, are concatenated into a single state vector Qfuii = Qactuai'J 
Qbase- This Combination of real and virtual joints into a single vector is called the 
generalized joint state vector. Let Udofs be the number real and virtual DOFs in 
the model that is used by WBOSC. Thus, Qfuii G 

The underactuation matrix U G defines the relationship between 

the actuated joint vector and the full joint state vector as shown by the following 
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equation. 

Qactual — U • Qfull ( 2 ) 

Let A be the robot’s generalized joint space inertia matrix, B be the generalized 
joint space Coriolis and centrifugal force vector, G be the generalized joint space 
gravity force vector, be the contact Jacobian matrix that maps from generalized 
joint velocity to the velocity of the constraint space dimensions, Ac be the co-state 
of the constraint space reaction forces, and Tcommand be the desired force/torque 
joint command vector that is sent to the robot’s joint-level controllers. The robot 
dynamics can be described by a single linear second order differential equation 
shown by the following equation. 

Af^base V 5 + G + jjAc = f ) (3) 

\Qactual / \^command/ 

Constraints are formulated as follows. Let Pc be the velocity of the constrained 
dimensions, which we approximate as being completely rigid and therefore yielding 
zero velocity on the contact points, as shown by the following equation. 

Pc = Jcf.®“^^)=0 (4) 

\Qactual / 

Tasks are formulated as follows. Let pi be the desired velocity of the task, Ji 
be the Jacobian matrix of task t that maps from generalized joint velocity to the 
velocity of the task space dimensions, and N^, be the generalized null-space of the 
constraint set. Furthermore, let be the contact consistent reduced Jacobian ma¬ 
trix of task t, i.e., it is consistent with U and Nc- The definition of pt is given by 
the following equation where operator arg is the dynamically consistent generalized 
inverse of arg. 

Pt = Jt( ) = JiUNcQactual 

\Qactual / 

— Jt Qactual ( 5 ) 

Let A( be the contact-consistent prioritized task-space inertia matrix for task 
t, Pt.ref be the reference, i.e., desired, task-space acceleration for task t, /JJ" be the 
contact-consistent task-space Coriolis and centrifugal force vector for task t, and Yt 
be the contact-consistent task space gravity force vector for task t. The force/torque 
command of task t, denoted Fj, is given by the following equation. 


Ft = h.*tpt,ref + Pt + it 


( 6 ) 


To achieve multi-priority control, let Jt\prev b® the Jacobian matrix of task t that 
is consistent with [/, fVc, and all higher priority tasks. The equation for Tcommand 
is the sum of all of the individual task commands multiplied by the corresponding 
Jt\prev matrix as shown by the following equation. 


'^command — 


t*T 

/ ^ ^t\prev 


Ft 


t 


(7) 
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Finally, when a robot has more than one point of contact with the environment, 
there are internal tensions within the robot. By dehnition, these “internal forces” 
are orthogonal to joint accelerations, i.e., they result in no net movement of the 
robot. The control structures like the multicontact/grasp matrix that are used to 
control these internal forces are documented in previous publications Let L* be 
the nullspace of (UNc) and Tintemai be the reference (i.e., desired) internal forces 
vector. The contribution of the internal forces can thus be added to Equation Q 
as shown by the following equation. 


^command — 


t 


{j:\lrerFt) + L* 


'^internal 


( 8 ) 


4. Controllt! Software Architecture 

There are six guiding principles behind Controlltl’s development: (i) separate con¬ 
cerns into interface definitions, implementations, and configuration, (ii) support 
extensibility and platform-independence through dynamically loadable plu¬ 
gins, (iii) encourage code reuse through plugin libraries, (iv) support systems 
integration through parameter binding, events, data introspection services, and 
compatibility with a modern software ecosystem, (v) be cognizant of performance 
and real- time considerations, and (vi) support two types of end users: de¬ 
velopers who use Controllt! and researchers who modify Controllt!. 


Section 4.1 contains a discussion of Controllt !’s software architecture, which de¬ 
scribes the software components within Controllt’s core. Many of these components 
either instantiate plugins or are implemented by plugins. The use of plugins enables 
Controllt! to be extensible in terms of supporting different robots and applications. 
Section |4.2| discusses mechanisms for configuring and integrating Controllt! into a 
larger system. This includes the parameter reflection, binding, and event signal¬ 
ing mechanisms, and YAML specification files. Finally, a description of Controlltl’s 
multi-threaded architecture is discussed in section IT^ 


4.1. Software Architecture 

The software abstractions that enable Controllt! to instantiate and integrate general 
WBOSC controllers are shown in Figurej^ The abstractions that are extendable via 
dynamically loadable plugins are colored gray. They include tasks, constraints, the 
whole body controller, the servo clock, and the robot interface. Non-extensible com¬ 
ponents include the compound task, robot model, constraint set, and coordinator. 
The coordinator implements the servo loop and uses all of the other abstractions 
except for the servo clock, which implements the servo thread and controls when 
the coordinator executes the next cycle of the servo loop. The software abstractions 
can be divided into three general categories: configuration, whole body control, and 
hardware abstraction. 

Configuration. Configuration software abstractions include the robot model, 
compound task, and constraint set. Their APIs and attributes are shown in Fig- 
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§ ServoClock 


§ RobotlnterPace 





Hardware Abstraction 


Fig. 1. The primary software abstractions within Controllt! consist of a compound task, con¬ 
straint set, robot model, whole body controller, servo clock, robot interface, and coordinator. The 
compound task contains a set of prioritized tasks. Tasks specify operational space or postural 
objectives and contain task-space controllers; multiple tasks may have the same priority level. 
Constraints specify natural physical constraints that must be satisfied at all times and are effec¬ 
tively higher priority than the tasks. The robot model computes kinematic and dynamic properties 
of the robot based on the current joint states. The servo clock and robot interface constitute a 
hardware abstraction layer that enables Controllt! to work on many platforms. The coordinator 
is responsible for managing the execution of the whole body controller. Arrows indicate usage 
relationships between the software abstractions. Abstractions that are dynamically extensible via 
plugins are colored gray. 


ure The robot model determines the kinematic and dynamic properties of the 
robot and builds upon the model provided by the Rigid Body Dynamics Library 
(RBDL)ii28|, which includes algorithms for computing forward and inverse kinemat¬ 
ics and dynamics and frame transformations. The kinematic and dynamic values 
provided by the model are only estimates and may be incorrect, necessitating the 
use of a whole body feedback controller. The robot model API includes methods 
for saving and obtaining the joint state and getting properties of the robot like the 
joint space inertia matrix and gravity compensation vector. There are also methods 
for obtaining the joint order within the whole body controller. A reference to the 
constraint set is kept within the robot model to determine which joints are vir¬ 
tual (i.e., the 6-DOF free floating joints that specify a mobile robot’s position and 
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Q CompoundTask 

tasks; Veaor [1] 

^ addTask(lask: Task) 

III updaie(moclel: RobotModel) 

getTaskJacobian(prlor1ty: Integer) : Matrix[l] 
m getTaskCommand(prlority: Integer): Vector|l| 
III getNumPnoridesO : lnteger[l] 


g rest 


1^ name : String [1] 

^ type: String |1| 
priority: Integer [1] 
isEnabled : Boolean [1] 

Ej inacdveState : TaskSIale (1) 

^ acdveState : TaskStaie [1] 

HI geiJacobian{); Matrix[l] 
gelCommandO : Vectorjl) 

HI update(model: RobotModel) 
m getNumDimensionsQ: inieger[l] 
HI swapAcdveStaieO 


§ RobotModel 

Ej; constraintSet: ConsffaintSei [1) 

Ej^ model: RigidBodyDynamics::Model [1] 

Ej; jointStaie : JoiniState [1] 

Eji inerdaMatiix: Matrix [1] 

Hi seiModel(model; RlgldBodyDynamlcs;:Model) 
Hi setJointState(jointState : JointState) 

HI updateQ 

Hi getModelQ ; RigidBodyDynamics::Model[l] 

0 geUointStateO: JointState|l| 

Hi getInertiaMatrixO : Malrix[l] 

0 geUoIntGraviTyQ : Vector[l] 

0 getConstralntSetO: ConstralntSetfl] 

0 getFullJointOrderO . V/ector[lJ 
0 getVIrtualJoIntOrderO : vector[l] 

0 geiReaUolntOrderO: Vector|i) 

0 geiActuatedJointOrderO: Vector|l| 


^ TaskState 


E^ taskJacoblan : Matrix [1] 
0 geUacobianO : Matr[x{l] 


§ JointState 


j| position : Vector [1] 

3 velocity; Vector |1] 

3 acceleration : Vector [1] 


Q ConstraintSet 

Ei constrainB: Vector [1] 

Ei Jacobian : Matrix [1] 

0 addConstraini(constraint; Constraint) 

0 isConstrarned(jolntName : String); Boolean[l] 
0 getUnderactuationMatrIxO : Uatrix[l] 

0 update(madel: RobotModel) 

0 geUacobianO: Uatrixfl] 

0 getlnverseJacobianQ : Matrixjl] 

0 getNullspaceO: Matrix|l] 

0 getInverseNullspaceQ : Uatr1x[l] 


B Constraint 

EJ name : String [I] 

^ type : Suing (ij 
E^ isEnabled : Boolean (1) 

0 getNumConstraIntedDOFsO ; lnleger[l] 
Jj|^^geUacobianOj^^atnx[^^^^^^^^^^^ 


B ContactConstraint 

B TransmissionConstraint 

B link: Siring (1] 

Q conlflctPoint: Vector [1] 

@ master Joint: String [1] 

Q slaveJoInts: Vector [1] 


Fig. 2. This UML diagram specifies the APIs of Controlltl’s configuration software abstractions. 
They are used to specify the objectives and constraints of the whole body controller. 


orientation within the world frame), real, and actuated. 

The compound task and constraint set contain lists of tasks and constraints, re¬ 
spectively. Tasks and constraints are abstract; concrete implementations are added 
to Controllt! through plugins. Both have names and types for easy identification 
and can be enabled or disabled based on context. A task represents an operational 
or postural objective for the whole body controller to achieve. Concrete task im¬ 
plementations contain goal parameters that, in combination with the robot model, 
produce an error. The error is used by a controller inside the task to generate a 
task-space effort commancQ which is accessible through the getCommandO method 
and may be in units of force or torque. In addition to the command, a task also 
provides a Jacobian that maps from task space to joint space. The compound task 
combines the commands and Jacobians of the enabled tasks and relays this informa¬ 
tion to the whole body controller. Specifically, for each priority level, the compound 
task vertically concatenates the Jacobians and commands belonging to the tasks 
at the priority level. The WBOSC algorithm uses these concatenated Jacobian and 
command matrices to support task prioritization and multiple tasks at the same 
priority level. 

Task Library. To encourage code reuse and enable support for basic applica¬ 
tions, Controllt! comes with a task library containing commonly used- tasks. The 
tasks within this library are shown in Figure There are currently six tasks in 
the library: joint position, 2D / 3D Orientation, center of mass, Cartesian position, 
and center of pressure. In the future, more tasks can be added to the library by 
introducing additional plugins. Of these, the joint position, orientation, and Carte¬ 
sian position tasks have been successfully tested in hardware. The rest have only 
been tested in simulation. Note that all of the tasks make use of a PIDController. 
This feedback controller generates the task-space command based on the current 


We use the word “effort” to denote generalized force, i.e., force or torque. 
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Fig. 3. This UML class diagram shows the tasks in Controlltl’s task library and the PID controller 
that they use. Combinations of these tasks specify the operational space and postural objectives 
of the whole body controller and collectively form the compound task. Concrete tasks are imple¬ 
mented as dynamically loadable plugins. Controllt! can be easily extended with new tasks via the 
plugin mechanism. 


error and gains. Alternative types of controllers like sliding mode control may be 
provided in the future. 

The joint position task directly specifies the goal positions, velocities, and accel¬ 
erations of every joint in the robot. It typically defines the desired “posture” of the 
robot, which is not an operational space objective but accounts for situations where 
there is sufficient redundancy within the robot to result in non-deterministic behav¬ 
ior when no posture is defined. Specifically, a posture task is necessary when the null 
space of all higher priority tasks and constraints is not nil, and the best practice is to 
always include one as the lowest priority task in the compound task. The joint posi¬ 
tion task has an input parameter called goalAcceleration to enable smooth tran¬ 
sitions between joint positions. The goal acceleration is a desired acceleration that 
is added as a feedforward command to the control law. The currentAcceleration 
output parameter is a copy of the goalAcceleration parameter and is used for 
debugging purposes. 

The 2D and 3D orientation tasks are used to control the orientation of a link on 
the robot. They differ in terms of how the orientations are specified. Whereas the 
2D orientation is specified by a vector in the frame of the body being oriented, the 
3D orientation is specified using a quaternion. The purpose of providing a 2D orien¬ 
tation task even though a 3D orientation could be used is to reduce computational 
overhead when only two degrees of orientation control is required. For example, a 
2D orientation task is used to control the heading of Trikey, a 3 wheeled holonomic 
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mobile robot, as shown in Figure [Tsl whereas a 3D orientation task is used to control 
the orientation of Dreamer’s end effectors, as shown in Figure IT^b). Visualizations 
of these two task-level controllers are given in [Appendix C| The 2D orientation 
task does not include a goalAngularVelocity input parameter because its current 
implementation assumes the goal velocity is always zero. This assumption can be 
easily removed in the future by modifying the control law to include a non-zero goal 
velocity. 

The Center Of Mass (COM) task controls the location of the robot’s COM, 
which is derived from the robot model. It is useful when balancing since it can 
ensure that the robot’s configuration always results in the COM being above the 
convex polygon surrounding the supports holding the robot up. The Center Of 
Pressure (COP) task controls the center of pressure of a link that is in contact 
with the ground. It is particularly useful for biped robots containing feet since 
it can help ensure that the COP of a foot remains within the boundaries of the 
foot thereby preventing the foot from rolling. The Cartesian position task controls 
the operational space location of a point on the robot. Typically, this means the 
location of an end effector in a frame that is specified by the user and is by default 
the world frame. For example, it is used to position Dreamer’s end effectors in front 
of Dreamer as shown in Figure As indicated by the figure, multiple Cartesian 
position tasks may exist within a compound task, as long as they control different 
points on the robot. 

As previously mentioned, the aforementioned tasks are those that are currently 
included with Controllt!. They are implemented as plugins that are dynamically 
loaded on-demand during the controller configuration process. Additional tasks may 
be added in the future. For example, an external force task may be added that con¬ 
trols a robot to assert a certain amount of force against an external obstacle. In 
addition, an internal force task may be added to control the internal tensions be¬ 
tween multiple contact points. A prototype of such a task was successfully used 
during NASA JSC DRC critical design revievj^to make Valkyrie to walk in sim¬ 
ulation, as shown in [Appendix C[ but is not included in the current task library 
due to the need for additional testing and refinement. For the walking behavior, 
Controlltl’s compound task included a COM Task, internal tensions task, posture 
task, and, for each foot, a COP, Cartesian position, and orientation task. 

Constraints. A constraint specifies natural physical limits of the robot. There 
are two types of constraints: ContactConstraint and TrcUismissionConstraint. 
Contact constraints specify places where a robot touches the environment. Trans¬ 
mission constraints specify dependences between joints, which occur when, for ex- 


a Track A DRC team, NASA JSC was required to undergo a critical design review by DARPA 
officials in June 2013, which was in the middle of the period leading up to the DRC Trials in 
December 2013. The results of the review determined whether the team would continue to receive 
funding and proceed to compete in the DRC Trials as a Track A team. NASA JSC was one of six 
Track A teams to pass this critical design review. 
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ample, joints are co-actuated. The parent Constraint class includes methods for 
obtaining the number of DOFs that are constrained and the Jacobian of the con¬ 
straint. Contact constraints have a get Joint () method that specifies the parent 
joint of the link that is constrained. Transmission constraints have a master joint 
that is actuated and a set of slave joints that are co-actuated with the master 
joint. Unlike tasks, constraints do not have commands since they simply specify 
the nullspace within which all tasks must operate. Like the compound task, the 
constraint set computes a Jacobian that is the vertical concatenation of all the con¬ 
straint Jacobians. In addition, it provides an update method that computes both 
the null space projector and UN^ (defined in Equation ^), accessors for these 
matrices, and methods for determining whether a particular joint is constrained. 
The whole body controller uses this information to ensure all of the constraints are 
met. While it is true that contact constraints are mathematically similar to tasks 
without an error term, we wanted to distinguish between the two since they serve 
significantly different purposes: tasks denote a user’s control objectives while con¬ 
straints denote a robot’s physical limits. We did not want to confuse the API by 
using the same software abstraction for both purposes. Furthermore, by separating 
tasks and constraints, the API will be easier to extend to support optimization 
based controllers with inequality constraints. 

Constraint Library. Constraints included in Controlltl’s constraint library 
are shown in Figure]^ Contact constraints include the flat contact constraint, omni 
wheel contact constraint, and point contact constraint. The flat contact constraint 
restricts both link translation and rotation. The omni wheel contact constraint 
restricts one rotational DOF and one translational DOF based on the current ori¬ 
entation of the wheel. Point contact constraint restricts just link translation. One 
transmission constraint called CoactuationConstraint is provided that enables 
Controllt! to handle robots with two co-actuated joints, like the torso pitch joints 
in Dreamer. It includes a transmission ratio specification to handle situations where 
the relationship between the master joint and slave joint is not one-to-one. Currently 
only the two-joint co-actuation case is supported, though a more generalized con¬ 
straint that supports more than two co-actuated joints could be trivially added 
in the future. Specifically, another child class of TrainsmissionConstraint can be 
added as a plugin to support the co-actuation of more than two joints by adding 
more rows to the constraint’s Jacobian. Like the task library, the constraint library 
can easily be extended with new constraints via the plugin mechanism used by 
Controllt!. 

Whole body control. The class diagrams for the whole body control soft¬ 
ware abstractions are shown in Figure]^ There are two classes: WBC and Com¬ 
mand. WBC is an interface that contains a single computeCommand() method. 
This method takes as input the robot model, which includes the constraint set, and 
the compound task. It performs the WBC computations that generate a command 
for each joint under its control and returns it within a Command object. The Com¬ 
mand object specifies the desired position, velocity, effort, and position controller 
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Fig. 4. This UML class diagram shows the constraints in Controlltl’s constraint library. Combina¬ 
tions of these constraints specify natural physical limits of the robot and constitute the constraint 
set. Concrete constraints are implemented as dynamically loadable plugins. Additional constraints 
can be easily added via the plugin mechanism. 


^ WBC 

m computeCommand(model : RobotModel, compoundTask: CompoundTask): Command[l] 


R Command 

[51 position ; Vector [1] 


[51 velocity: Vector [1] 


H effort: Vector [1] 


[51 positionKp : Vector [1] 


[51 positionKd : Vector [1] 



Fig. 5. The WBC software abstractions within Controllt! consist of an interface called WBC and a 
class called Command. The WBC interface defines a single method called computeCommand that 
takes two input parameters, the robot model, which includes the constraint set, and the compound 
task. It returns a Command object. The command includes position, velocity, effort, and position 
controller gains. Depending on the type of joint controller used, one or more of the member 
variables inside the command may not be used. For example, a pure force or torque-controlled 
robot will only use the effort specification within the command. 


gains. Note that not all of these variables need to be used. For example, a robot 
that is purely effort controlled will only use the effort command. The optional fields 
within the command are included to support robots with joints that are position 
or impedance controlled. 

The whole body controller within Controllt! is dynamically loaded as a plugin 
using ROS pluginlibE^. Two plugins are currently available as shown in Figure 
They include WBOSC and WBOSCJmpedance. The WBOSC plugin implements 
the WBOSC algorithm. It computes the nullspace of the constraint set and projects 
the task commands through this nullspace. Task commands are iteratively included 
into the final command based on priority. The commands of tasks at a particular 
priority level are projected through the nullspaces of all higher priority tasks and 
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Q WBC 

0 computeCommand(model : RobotModel, compoundTask : CompoundTask): Command[l] 

- z - 


y WBOSC 

higherPriorityNullspace : Matrix [1] 



taskGravityComp : Vector [1] 

Ei higherPriorityEffort : Vector [1] 
massinertia: Matrix [1] 

m computeComrtiand(model : RobotModel, compoundTask : CompoundTask) : Command[l] 


Fig. 6. Controllt! currently includes two plugins in its WBC plugin library. They consist of WBOSC 
and WBOSCJmpedance. WBOSC implements the actual WBOSC algorithm that takes a holistic 
view of the robot and achieves multiple prioritized task objectives using nullspace projection. It 
outputs a pure effort command and is use with effort-controlled robots like Dreamer. The second 
plugin, WBOSC-Impedance, extends WBOSC with an internal robot model that specifies the 
desired joint positions and velocities based on the torque commands generated by WBOSC. This 
is useful to support robots with joint impedance controllers, an example of which is NASA JSC’s 
Valkryie. 


the constraint set. This ensures that all constraints are met and that higher priority 
tasks override lower priority tasks. The output of WBOSC is an effort command that 
can be sent to effort controlled robots like Dreamer. The member variables within 
the WBOSC plugin ensure that memory is pre-allocated, which reduces execution 
time jitter and thus increases real-time predictability. 

To support impedance-controlled robots, Controllt! also comes with the 
WBOSCJmpedance plugin. Unlike effort-controlled robots, impedance-controlled 
robots take more than just effort commands. Specifically, in addition to effort, 
impedance controllers also take desired position and velocity commands, and option¬ 
ally position controller gains when controller gain scheduling is desired. The benefit 
of using impedance control is the ability to attain higher levels of impedance. This is 
possible since the position and velocity control loop can be closed by the embedded 
joint controller, which typically has a higher servo frequency and lower communi- 
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cation latency than the WBC controller. The WBOSCJmpedance plugin extends 
the WBOSC plugin with an internal model that converts the effort commands gen¬ 
erated by the WBOSC algorithm into expected joint positions and velocities. The 
member variables within the WBOSC_Impedance plugin that start with “qi_” hold 
the internal model’s joint states. The prevUpdateTime member variable records 
when this internal model was last updated. Each time computeCommand is called, 
WBOSCJmpedance computes the desired effort command using WBOSC. It then 
uses this effort command along with the robot model to determine the desired accel¬ 
erations of each joint. WBOSCJmpedance then updates the internal model based 
on these acceleration values, the time since the last update, the previous state of 
the internal model, and the actual position and velocity of the joints. The derived 
joint positions, velocities, and efforts are saved within a Command object, which is 
returned. This control strategy was used on the upper body of NASA JSC’s Valkyrie 
robot to perform several DRC manipulation tasks as previously mentioned. 

Hardware abstraction. To enable support for a wide variety of robot plat¬ 
forms, Controllt! includes a hardware abstraction layer consisting of two ab¬ 
stract classes, the Robotinterface and the ServoClock, as shown in Figure 
Concrete implementations are provided through dynamically loadable plugins. 
Robotinterface is responsible for obtaining the robot’s joint state and sending 
the command from the whole body controller to the robot. For diagnostic purposes, 
it also publishes the state and command information onto ROS topics using a real¬ 
time ROS topic publisher, which uses a thread-pool to offload the publishing process 
from the servo thread. ServoClock instantiates the servo thread and contains a ref¬ 
erence to a Controller, which is implemented by the Coordinator. ServoClock is 
responsible for initializing the controller by calling servoInitO and then periodi¬ 
cally executing the servo loop by calling the servoUpdateO method. Initialization 
using the actual servo thread is needed to handle situations where certain initial¬ 
ization tasks can only be done by the servo thread. This occurs, for example, when 
the servo thread is part of a real-time context meaning only it can initialize certain 
real-time resources. 

Controllt! includes libraries of Robotinterface and the ServoClock plugins 
as shown in Figure Robotinterface plugins include general ones that commu¬ 
nicate with a robot via three different transport layers: ROS topics (Robotlnter- 
faceROSTopic), UDP datagrams (RobotIntefaceUDP), and shared memory (Robot- 
InterfaceSM). These are meant for general use - Controllt! includes generic Gazebo 
plugins and abstract classes that facilitate the creation of software adapters for al¬ 
lowing simulated and real robots to communicate with Controllt! using these three 
transport layers. Among the three transport layers, shared memory has the lowest 
latency and is most reliable in terms of message loss. It uses the ROS shared mem¬ 
ory interface package lJU*, which is based on boost’s interprocess communication 
library. 

In addition to general Robotinterface plugins, Controllt! also includes two 
robot-specific plugins, one for Dreamer (RobotinterfaceDreamer), and one for 
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Q Robotinterface 


Q ServoClock 

commandPublisher: RealTimeROSTopicPublisher [1] 
statePublisher: RealTimeROSTopicPublisher [1] 

^ read(block : Boolean): RobotState[1] 
write(command : Command) 


frequency : Float [1] 
callServolnit: Boolean [1] 
continueRunning : Boolean [1] 
controller: Controller [1] 
init(controller: Controller) 



§ RobotState 


start(frequency: Float) 

% stopO 

updateLoopO 

O jointPosition : Vector [1] 

(□1 jointVelocity : Vector [1] 


B jointAcceleration : Vector [1] 



B jointEffort: Vector [1] 

Ol vlrtualJointPosition : Vector [1] 

B virtualJointVelocity : Vector [1] 


^ Controller 


^ servolnItO 
servoUpdateO 


Fig. 7. Controllt! employs a hardware abstraction layer that consists of a Robotinterface and 
a Clock. The Robotinterface has two methods: read and write. The read method returns a 
RobotState object that includes details about the robot joint positions, velocities, accelerations, 
and efforts. The write method takes as input a Command object and issues the command to the 
robot joints. 



Fig. 8. The robot interface plugins that are currently available include support for the following 
transport protocols: ROS Topic, UDP, and shared memory. There are also specialized robot inter¬ 
faces for Dreamer and Valkyrie. The servo clocks provided include support for std: :chrono, ROS 
time, and RTAI time. 


Valkyrie (RobotinterfaceValkyrie). RobotInterfaceDreamer interfaces with a 
RTAI real-time shared memory segment that is created by the robot’s software 
interface called the M3 Server. It also implements separate PID controllers for 
robot joints that are not controlled by WBC. They include the finger joints 
in the right hand, the left gripper joint, the neck joints, and the head joints. 
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In the current implementation, these joints are fixed from WBC’s perspective. 
RobotinterfaceValkyrie interfaces with shared memory segment created by 
Valkyrie’s software interface. This involves integration with a controller manager 
provided by ros_control ^ to gain access to robot resources. 

Controllt! includes several ServoClock plugins to enable flexibility in the 
way the servo thread is instantiated and configured to be periodic. The current 
ServoClock plugin library includes plugins for supporting servo threads based on a 
ROS timer, a C++ std: :chrono timer, or an RTAI timer. Support for additional 
methods can be included in the future as additional plugins. 


4.2. Configuration and Integration 

Support for configuration and integration is important because as a software frame¬ 
work Controllt! is expected to be (1) used in many different applications and hard¬ 
ware platforms that require different whole body controllers and (2) just one com¬ 
ponent in a complex application consisting of many components. In addition, Con- 
trolIt!’s configuration and integration capabilities directly impacts the software’s 
usability, which must be high to achieve the goal of widespread use. Controllt! 
supports integration through four mechanisms: (1) parameter reflection, which 
exposes controller parameters to other objects within Controllt! and is used by the 
other two mechanisms, (2) parameter binding, which enables the parameters to 
be connected to external processes through an extensible set of transport layers, 
(3) events, which enable parameter changes to trigger the execution of external 
processes without the use of polling, and (4) services, which enable external pro¬ 
cesses to query information about the controller. Controllt! supports configuration 
through scripts that enable users to specify the structure of the compound task 
and constraint set, the type of whole body controller and hardware interface to use, 
the initial values of the parameters, the parameter bindings, and the events. These 
scripts are interpreted during Controllt!’s initialization to automatically instanti¬ 
ate the desired whole body controller and integrate it into the rest of the system. 
Details of ControlIt!’s support for configuration and integration are now discussed. 

Parameter Reflection. Parameter reflection was originally introduced in 
Stanford-WBC. It defines a ParameterRefiection parent class through which child 
class member variables can be exposed to other objects within Controllt!. The API 
and class hierarchy of the ParameterRefiection class is shown in Figure]^ (a). Pa¬ 
rameter reflection enables internal control parameters to be exposed to other objects 
within Controllt!. It consists of an abstract parent called ParameterRefiection that 
provides methods for declaring and looking up parameters. When a parameter is de¬ 
clared, it is encapsulated within a Parameter object, which contains a name, pointer 
to the actual variable, a list of bindings, and a method to set the parameter’s value. 
Subclasses of ParameterRefiection are able to declare their member variables as pa¬ 
rameters and thus make them compatible with Controllt’s parameter binding and 
event mechanism. 
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Parameter Binding. Parameter binding enables the integration of Controllt! 
with other processes in the system by connecting parameters to an extensible set of 
transport layers. Its API and class hierarchy is shown in Figure [^(b). The classes 
that constitute the parameter binding mechanism consist of a BindingManager that 
maintains a set of BindingFactory objects that actually create the bindings, and a 
BindingConf ig object that specifies properties of a binding. The required properties 
include the binding direction (either input or output), the transport type, which is a 
string that must match the name of a Binding provided by a BindingFactory plugin, 
and a topic to which the parameter is bound. The BindingConf ig also contains an 
extensible list of name- value properties that is transport protocol specific. For 
example, transport- specific parameters for ROS topic output bindings include the 
publish rate, the queue size, and whether the latest value published should be 
latched. 

During the initialization process, BindingConf ig objects are stored as param¬ 
eters within a ParameterReflection object, which is passed to the BindingMan¬ 
ager. The BindingManager searches through its BindingFactory objects, which are 
dynamically loaded via plugins, for factories that are able to create the desired 
binding. The current bindings in Controllt’s binding library include input and out¬ 
put bindings for ROS topics and shared memory topics. More can be easily added 
in the future via the plugin architecture. The newly created Binding objects are 
stored in the parameter’s Parameter object. When a parameter’s value is set via 
Parameter. set 0, the new value is transmitted through output bindings to which 
the parameter is bound. This enables changes in Controllt! parameters to be pub¬ 
lished onto various transport layers and topics notifying external processes of the 
latest values of the parameters. Similarly, when an external process publishes a value 
onto a transport layer and topic to which a parameter is bound via an input bind¬ 
ing, the parameter’s value is updated to be the published value. This enables, for 
example, external processes to dynamically change a task’s references or controller 
gains, which is necessary for integration. 

Events. Events contain a logical expression over parameters that are interpreted 
via muParser an open-source math parser library. Its API is shown in Figure 
(c). Events are stored in the ParameterReflection parent class. The servo thread 
calls ParameterReflection.emitEvents() at the end of every servo cycle. The names 
of events whose condition expression evaluates to true are published on ROS topic 
/[controller name]/events. Events contain a boolean variable called “enabled” 
that is used to prevent an event from continuously firing when the condition expres¬ 
sion remains true since this would likely flood the events ROS topic. Instead, events 
maintain a fire-once semantic meaning they only fire when the condition expression 
changes from false to true. 

Service-based controller introspection capabilities. To further assist Con¬ 
trollt! integration, into a larger system, Controllt! also includes a set of service- 
based introspection capabilities. Unlike ROS topics, which are asynchronous uni¬ 
directional, ROS services are bi-directional and synchronous. Controllt! uses this 
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§ BindingManager 

iBi bindingFactories : Vector [ij 
m bindParameters(parameters: ParameterReflectlon) 


§ BindingConfig 


1^ direction : String [1] 

El transporlType : String [1] 
lEjj topic : String II] 

IBI properties; Table [1] 

^ addProperty(name : String, value : String) 


§ BIndIngFactory 


m createBinding(BindingConfig : BindingConfig, parameter: Parameter): Binding[l] 


I 

§ BindingFactoryROS 


I 

§ BindingFactorySM 


§ Binding 


ij config : BindingConfig [1] 
j) parameter: Parameter [l] 


§ InputBindingROS 

P OutputBindingROS 

P InputBindingSM 

El sub : roscSubscriber [1] 

El pub : ros::Publisher [1] 

. ^ sub : smcSubscriber [1] 


(b) Parameter Binding 


§ OutputBindingSM 


i;:Publisher[l] 


Fig. 9. Controllt! includes three mechanisms for integration: parameter reflection, parameter bind¬ 
ing, and events. Sub-figure (a) shows the parameter reflection mechanism that enables parameters 
to be exposed to other objects within Controllt! including the parameter binding and event mech¬ 
anisms. Sub- figure (b) shows the parameter binding mechanism that enables parameters to be 
bound to an extensible set of transport layers, which enables them to be accessed by external 
processes. Sub-figure (c) shows an event definition. Events are stored within ParameterRefiection 
objects and are emitted at the end of the servo loop. They enable external processes to be no¬ 
tified when a logical expression over a set of parameters transitions from being false to true and 
eliminates the need for external processes to poll for state changes within Controllt!. 


capability to enable external processes to query certain controller properties 
as it is running. For example, two often- used services include / [controller 
name]/diagnostics/getTaskParcuneters, which returns a list of all tasks in the 
compound task, the parameters, and their parameter values, and / [controller 
name]/diagnostics/getRealJointlndices, which returns the ordering of all real 
joints in the robot. This is useful to determine the joint order when updating the 
reference positions of a posture task or interpreting the meaning of the posture 
task’s error vector. A full list of Controllt’s service-based controller introspection 
capabilities is provided in [Appendix C[ 

Script-based configuration and initialization. As previously mentioned, 
Controllt! supports script-based configuration specification and initialization en¬ 
abling integration into different applications and platforms without being recom¬ 
piled. This is necessary given the plethora of properties that must be defined and 
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the wide range of anticipated applications and hardware platforms. To instantiate 
a whole body controller using Controllt!, the user must specify many things includ¬ 
ing the compound task, constraint set, whole body controller, robot interface, servo 
clock, initial parameter values, parameter bindings, and events. In addition, there 
are numerous controller parameters as defined in Appendix [Appendix B| Controllt! 
enables users to define the primary WBC configuration and integration abstractions 
including tasks, constraints, compound tasks, constraint set, parameter bindings, 
and events via a YAML file whose syntax is given in Appendix [Appendix D| The 
remaining parameters are defined through the ROS parameter server, which can 
also be initialized via another YAML file that is loaded via a ROS launch filelil^l. 
ROS launch is actually a powerful tool for loading parameters and instantiating pro¬ 
cesses. Controllt! leverages this capability to enable users to initialize and execute 
a Controllt! whole body controller using a single command. 


4.3. Multi-threaded Architecture 


Higher servo frequencies can be achieved by decreasing the amount of computation 
in the servo loop. The amount of computation can be reduced because robots typ¬ 
ically move very little during one servo period, which is usually 1ms. Thus, state 
that depends on the robot configuration like the robot model and task Jacobians 
often do not need to be updated every servo cycle. Controllt! takes advantage of this 
possibility by offloading the updating of the robot model and the task states, which 
include the task Jacobians, into child threads. Specifically, Controllt! uses three 


threads as shown in Figure 10 They include (1) a Servo thread that executes the 


actual servo loop, (2) a ModelUpdater thread that is responsible for updating the 
robot model, which includes the kinematics, inertia matrix, gravity compensation 
vector, the constraint set, and the virtual linkage model, and (3) a TaskUpdater 
thread that is responsible for updating the states of each task in the compound 
task, which includes the task Jacobians. The Servo thread is instantiated by the 
ServoClock and can thus be real-time when, for example, ServoClockRTAI is used. 
ModelUpdater and TaskUpdater are child threads that do not operate in a real¬ 
time manner. From a high-level perspective. Servo provides ModelUpdater with the 
latest joint states. The ModelUpdater uses this information to update the robot 
model in parallel with the Servo thread, and provides the updated robot model to 
the Servo when complete. Whenever the robot model is updated, the Servo thread 
provides the updated model to the TaskUpdater thread, which updates the task 
states. These updated task states are then provided to the Servo thread. Details 
on how this process is achieved in a manner that is non-blocking and safe are now 
discussed. 

Two key requirements of the multi-threaded architecture are (1) the Servo 
thread must not block and (2) there must not be any race conditions between 
threads. The first requirement implies that the servo thread cannot call the block¬ 
ing lock() method on the mutexes protecting the shared states between it and the 
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Fig. 10. To achieve higher servo frequencies, Controllt! employs a multi- threaded architecture 
consisting of three threads: (a) Servo, (b) ModelUpdater, and (c) TaskUpdater. Servo is a real¬ 
time thread whereas ModelUpdater and TaskUpdater are non-real- time threads. The names are 
self-descriptive. This figure shows the behavior and interactions of these threads. At a high level, 
Servo gives ModelUpdater the latest joint states and receives an updated robot model. It also gives 
TaskUpdater an updated robot model and receives updated state for each task, which includes 
the task Jacobians. To prevent Servo from blocking due to contention between it and the other 
threads, which is necessary for real-time operation, Controllt! maintains two copies of the robot 
model and two copies of the state for each task - an “active” one and an “inactive” one. Active 
versions are used solely by Servo. Inactive versions are updated by the child threads. To get 
updates from the child threads, Servo swaps the active and inactive versions when it can be done 
in a non-blocking and safe manner. It does this by calling the non-blocking tryLockO operation 
on the mutex protecting the inactive version of the robot model and only performing the swap 
when it successfully obtains the lock. The swapping of task state is kept non-blocking and safe 
through FSM design — a task will only indicate it has updated state after the TaskUpdater thread 
is done updating it. To prevent contention between the child threads, the inactive and active 
robot models can only be swapped when TaskUpdater is idle. To further reduce unnecessary 
computations, TaskUpdater only executes after the robot model is swapped. 


child threads. Instead, it can only call the non-blocking try_lock() method, which 
returns immediately if the lock is not obtainable. Controllt !’s multi-threaded ar¬ 
chitecture is thus structured to only require calls to try_lock() within the Servo 
thread. To prevent race conditions between threads, two copies of the robot model 
and task state are maintained: an “active” copy that is used by the Servo thread, 
and an “inactive” one that is updated by the non-servo threads. Updates from the 
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child threads are provided to the Servo thread by swapping the active and inactive 
states. This swapping is done by the Servo thread in a non-blocking and oppor¬ 
tunistic manner. 

Figures 10 (a) and (b) show how the Servo thread passes the latest joint state 
information to the ModelUpdater thread and trigger it to execute. After obtaining 
the latest joint states by calling Robotlnterface.read() and checking for updates 
from the child threads by executing the CheckForUpdates FSM, the Servo thread 
attempts to obtain the lock on the mutex protecting the inactive robot model by 
calling ModelUpdater. tryLockO . If it is able to obtain the lock on the mutex, 
it saves the latest joint states in the inactive robot model and then triggers the 
ModelUpdater thread to execute by calling ModelUpdater.unlockAndUpdateO. 
As the name of this method implies, the Servo thread also releases the lock on the 
inactive model thereby allowing the ModelUpdater thread to access and update the 
inactive robot model. If the Servo thread fails to obtain the lock on the inactive 
model, the ModelUpdater thread must be busy updating the inactive model. In this 
situation, the Servo thread continues without updating the inactive model. 

To prevent race conditions between the Servo thread and the child thread, up¬ 
dates from child threads are opportunistically pulled by the Servo thread. This is 
because the child threads operate on inactive versions of the robot model and task 
states, and only the Servo thread can swap the active and inactive versions. There 
are two points in the servo loop where the Servo thread obtains updates from the 
child threads. This is shown by the two “CheckForUpdates” states in left side of 
Figure (a). They occur immediately after obtaining the latest joint states by 
calling RobotInterface.read(), and immediately after triggering the ModelUpdater 
thread to run or failing to obtain the lock on the inactive robot model. More checks 
for updates could be interspersed throughout the servo loop but we found these two 
placements to be sufficient. 

The operations of the CheckForUpdates state are shown in the upper-right cor¬ 
ner Figure IT The Servo thread first obtains task state updates and then checks 
whether the TaskUpdater thread is idle. If it is idle, the Servo thread again checks 
for updated task states. This is to account for the following degenerate thread in¬ 
terleaving during the previous check for updated task states that would result in 
the permanent loss of updated task state: 


(1) The Servo thread begins to check some of the tasks for updated states. 

(2) TaskUpdater thread updates all of the tasks including those that were just 
checked by the Servo thread and returns to idle state. Note that this is pos¬ 
sible even if the Servo thread is real-time and has higher priority since the 
TaskUpdater may be executing on a different CPU core. 

(3) The Servo thread completes checking the remainder of the tasks for updates. 


In the above scenario, the tasks that were checked in step 1 would have updated 
states that would be lost without the Servo-thread re-checking for them after it 
confirms that the TaskUpdater is idle. In a worst-case scenario, the TaskUpdater 
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thread may update all of the tasks after the Servo thread checks for updates but 
before it checks whether the TaskUpdater is idle, resulting in the loss of updated 
state from every task. The loss of updated task state is not acceptable despite the 
presence of future update rounds since it is theoretically possible for the updated 
states of the same tasks to be continuously lost during every update round. While 
seemingly improbable, this “task update starvation” problem was actually observed 
and thus discovered while testing Controllt! on Valkyrie. 

After verifying that the TaskUpdater thread is idle and ensuring all of the 
updated task states were obtained, the Servo thread next checks for an updated 
robot model by calling ModelUpdater. checkUpdate (). This method switches to 
the updated robot model if one is available. If the model was updated, the 
Servo thread then calls TaskUpdater .updateTasksO passing it the updated robot 
model. This method is non-blocking since the TaskUpdater must be idle. It trig¬ 
gers the TaskUpdater to update the states of each task in the compound task. 
Note that if the robot model was not updated, the Servo thread does not call 
TaskUpdater .updateTasksO since task state updates are based on changes in the 
robot model. 

The current implementation does not consider the possibility that the active 
robot model or task states become excessively stale. This can occur if the robot 
moves so quickly that the model changes significantly since the last time it was 
updated. Controllt’s multi-threaded architecture can be easily modified to monitor 
difference between the current robot state and the robot state that was used to 
update the currently-active robot model and task states. If the difference exceeds 
a certain threshold, the Servo loop can update the active model itself to prevent 
excessive staleness. We currently do not implement this because our evaluations did 
not indicate the need for it. 

Sometimes a multi-threaded architecture is not necessary when the robot has 
a limited number of joints, the control computer is particularly fast, and the com¬ 
pound task is structured to reduce computational complexity (e.g., by using simpler 
tasks or limiting the number of tasks that share the same priority level). In this 
case, Controlltl’s multi-threaded architecture can be disabled by setting two ROS 
parameters, single_threaded_model and single_threaded_tasks, to be true prior 
to starting Controllt!. Details of these parameters are given in Table which is 
in [Appendix B| When these parameters are set to true, the servo loop updates the 
model and task states each cycle of the servo loop. 

Regardless of whether a multi-threaded architecture is used, the servo loop must 
be executed in a real-time manner. To help facilitate this, no dynamic memory al¬ 
location can occur once the servo loop starts. The initialization process consists of 
instantiating all objects using their constructors and then calling initO methods 
on all of the objects. All necessary memory is allocated during either the construc¬ 
tion or initialization phases. To ensure no memory is being dynamically allocated 
in the linear algebra operations that are extensively used in WBOSC, we tested 
the code by defining the EIGEN_RUNTIME_N0_MALL0C preprocessor macro prior to 
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Fig. 11. This sequence of snapshots show the movements of Dreamer performing a product disas¬ 
sembly task. Initially a metal pipe with a rubber valve is in front of Dreamer. To disassemble the 
product, Dreamer grabs the pipe with her right hand while using her left gripper to remove the 
valve. The pipe and valve are then placed into separate containers for storage. This demonstrates 
the integration of Controllt! with a robot and an application, and the fact that the task and 
constraint libraries are sufficiently expressive to accomplish this task. 


including the Eigen headers. 

5. Evaluation 

We integrate Controllt! with Dreamer, a dual-arm humanoid upperbody made by 
Meka Robotics, which was purchased by Google in December 2013. Dreamer’s arms 
and torso contains series elastic actuators and high fidelity torque control. The robot 
is modeled as a (16 -I- 6 = 22) DOF robot where 16 are the physical joints and the 
remaining 6 DOFs represent the floating DOFs|^ 

5.1. Product Disassembly Application 

Using Controllt!, we developed an application that makes Dreamer disassemble 
a product. A sequence of snapshots showing Dreamer performing the task using 
Controllt! is given in Figure El The task is to take apart an assembly consisting of 
a metal pipe with a rubber valve installed at one end. To remove the valve, Dreamer 
is programed to grab and hold the metal pipe with her right hand while using her 
left gripper to detach the valve. Once separated. Dreamer places the two pieces into 
separate storage containers. 

Two compound task configurations were used to achieve the product disassembly 
task: 

(1) single priority level containing a joint position task 

(2) dual priority level containing two higher priority Cartesian position tasks and 
two 2D orientation tasks (one for each wrist) and a lower priority posture task. 

The benehts of the second conhguration are shown by demonstrating how chang¬ 
ing just three controller parameters, i.e., the Cartesian position of the product, en¬ 
ables the controller to adapt to changes in the product’s location while continuously 


®WBOSC by default always assumes a floating base. In the case when the robot is fixed in place, 
it is represented in WBOSC as a constraint. This enables Controllt! to be more generic in terms 
of supporting both mobile and fixed robots. 
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Fig. 12. Controllt! is integrated into a larger system consisting of three major components; Con¬ 
trollt!, the application, and a data logger. Each of these components run as a separate process but 
communicate over ROS topics, which are represented by the arrows. The ROS topics are bound 
the variables within Controllt!. The WBOSC configuration consists of two priority levels within 
the compound task is shown. Higher priority numbers correspond to higher priority tasks. The 
other components within Controllt! are not shown since they do not have any bound parameters 
in this application. 


minimizing the squared error of the posture task. This is in the spirit of WBC where 
changes in a low-dimensional space (three Cartesian dimensions) results in desirable 
changes in a larger dimensional space (e.g., the number of DOFs in the robot). 

Developing the product disassembly application required writing new 
Robotinterface and ServoClock plugins that enable Controllt! to work with 
Dreamer. This is because Dreamer comes with the M3 software that is designed 
specifically for robots built by Meka. The M3 software includes the M3 Server, 
which instantiates an RTAI shared memory region through which Controllt! can 
transmit torque commands and receive joint state information. In addition, the 
M3 Server also implements the transmissions that translate between joint space 
and actuator space and the protocol for setting the modes and gains of the joint 
controllers executing on the robot’s DSPs. Other useful tools provided by the M3 
software include applications for tuning and calibrating individual joints. The Con- 
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Property 

Control PC 

Application PC 

CPU 

Intel Core 17-4771 @ 3.56CHz 

Intel Core 17-4771 @ 

3.56CHZ 

Motherboard 

Zotac H87 

JetWay JNP9J-Q87 

os 

Ubuntu 12.04 server, 32-bit, kernel 

2.6.32.20, RTAI 3.9, EtherCAT 1.5.1 

Ubuntu 14.04 desktop, 

64-bit, Kernel 3.13.0-44 

Middleware and 
Applications 

ROS Hydro, Controllt!, M3 Server 

ROS Indigo, demo 
applications, Gazebo 


(b) 


Fig. 13. The system consists of a humanoid robot that’s connected to a control PC over a 100Mbps 
EtherCAT network. The control PC runs Controllt! and is connected to an application PC over 
a two-hop IGbps Ethernet network. The application PC runs the application, which remotely 
interacts with Controllt! via ROS topics. Details of the hardware and software on the control and 
application PCs are given in the table. Note that the control PC runs an older operating system 
and older middleware than the application PC despite having similar hardware. This is because 
configuring the control PC for real-time operation while remaining compatible with the robot 
hardware is difficult. Allowing applications to run on a separate PC enables them to operate in 
a more up-to- date software environment and reduces the likelihood of interference between the 
applications and the controller. 


trollt! robot interface we developed for Dreamer is called RobotInterfaceDreamer. 
It uses the shared memory region created by the M3 Server to connect the WBOSC 
controller to the robot, and implements separate simpler controllers for the joints 
that are not controlled by WBOSC. These joints include the finger joints in the 
right hand, the left gripper joint, the neck joints, and the head joints (eyes and 
ears). In the current implementation, these joints are fixed in place from WBOSC’s 
perspective. While this is not true, they are located at the robot’s extremities and 
are attached to relatively small masses; the feedback portion of the WBOSC con¬ 
troller is able to sufficiently account for these inaccuracies as demonstrated by the 
successful execution of the application. 

Because Dreamer’s M3 software is designed to work with RTAI we cre¬ 
ated an RTAI-enabled servo clock called ServoClockRTAI, which instantiates a 
RTAI real-time thread for executing the servo loop within Controllt!. Whereas 
RobotinterfaceDreamer is specific to Dreamer, ServoClockRTAI can be re-used 
on any robot that is RTAI-compatible to get real-time execution semantics. 

Since Dreamer contains a 2-DOF torso and two 7-DOF arms, we use a compound 
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task containing a Cartesian position and orientation task for each of the two end 
effectors, and a lower priority joint position task for defining the desired posture. 
The constraint set contains two constraints: a FlatContactConstraint for fixing 
the robot’s base to the world and a CoactuationConstraint for the upper torso 
pitch joint that is mechanically connected to the lower torso pitch joint by a 1:1 
transmission. This results in the positions and velocities of the two joints to always 
be the same. The Jacobian of the CoactuationConstraint consists of one row and a 
column for each DOF in the robot’s model. The column representing the slave joint 
contains a 1 and the column representing the master joint contains the negative of 
the transmission ratio. Details of these types of constraints were discussed in^^. 

Finally, the goal state and error of every task in the compound task are bound 
to ROS topics so they can be accessed by the application. A data logger based on 
ROSBaglii^ is used to record experimental data. Figure 1^ shows how the various 
components are connected. Kinesthetic teaching is used to obtain the trajectories 
for performing the task, which consists of manually moving the robot along the de¬ 
sired trajectories while taking snapshots of the robot’s configuration. Cubic spline 
is used to interpolate intermediate points between snapshots. Note that the appli¬ 
cation is open-loop in that the robot does not sense where the metal pipe and valve 
assembly is located. We manually reposition the metal pipe and valve assembly at 
approximately the same location prior to executing the application. 

Before the application can be successfully executed, calibration and gain tuning 
must be done for every joint and controller in the system. We calibrated and tuned 
one joint at a time starting from those in the robot’s extremities (e.g., wrist yaw 
joints) and moving inward to joints with increasing numbers of child joints. Once 
all of the joints were calibrated and torque controller gains tuned, we proceeded 
to tune the task-level gains in the following order: joint position task, Cartesian 
position tasks, and finally orientation tasks. The gains used are given in [Appendix] 
[E| Note that these gains are dependent on Controllt’s servo frequency, which we 
set to be IkHz, and the end-to-end communication latency between the whole body 
controller and the joint torque controllers, which is about 7ms. 

The system architecture is shown in Figure It consists of the robot, the 
control PC, and the application PC. The robot communicates with the control PC 
over a 100Mbps EtherCAT link. The control PC communicates with an application 
PC via a 2-hop IGbps Ethernet network. The control PC runs Controllt! on an 
older but real-time patched version of Linux relative to the application PC. This 
is because upgrading the operating system on the control PC while maintaining 
compatibility with RTAI and necessary drivers like EtherCAT and ensuring accept¬ 
able real-time performance is a difficult and time consuming process that requires 
extensive testing. The product disassembly application could run directly on the 
Control PC, but we chose to run in on a different application PC to emphasize the 
ability to integrate Controllt! with remote processes and to allow the application to 
make use of a newer operating system, middleware, and libraries. In addition, run¬ 
ning the application on a separate PC reduces the likelihood that the application 
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Fig. 14. Performance data collected from one execution of the product disassembly application. 


would interfere with the whole body controller especially if the application includes 
a complex GPU-accelerated GUI. 

The application PG includes the dynamics simulator Gazebo When devel¬ 
oping the product disassembly application, we always tested the application in sim¬ 
ulation prior to on real- hardware, reducing the number of potentially-catastrophic 
problems encountered on hardware. For example, on the real hardware, if the ap¬ 
plication crashes while the arms are above the table, the arms may slam into the 
table with enough force to result in damage to the robot and perhaps the table. 
Testing the application in simulation enabled us to evaluate application stability. 
We implemented the application in Python (see Appendix F for an example code 
fragment), which further increases the importance of simulation testing since there’s 
no compilation stage to identify potential problems. Note that the application could 
have been written in any programming language supported by ROS Because 
Gontrollt! has a hardware abstraction layer consisting of a Robotinterface plugin 
and a ServoGlock plugin, switching between testing the application in simulation 
versus on the real hardware is simple and does not require any changes to the code. 

After tuning the controllers, we were able to repeatedly execute the application 
in a reliable manner. Figure shows performance data collected from one of the 
many executions of the application. The data was collected from ROS topics to 
which internal controller parameters were bound. Average statistics are given in 
Table The results show average servo computational latencies of about 0.5ms, 
which is the amount of time the servo thread takes to compute one cycle of the servo 
loop and is an order of magnitude faster than the 5ms achieved by UTA-WBC. 
Table shows the results of an experiment that obtains a detailed breakdown of 
the latencies within the servo loop by instrumenting the servo loop with timers. The 
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Statistic 

Sample Size Average 

Units 

Right Hand Cartesian Error 

49,137 

2.79 ± 0.56 

cm 

Right Hand Orientation Error 

55,735 

3.72 ± 3.12 

degrees 

Left Hand Cartesian Error 

43,026 

1.91 ± 0.67 

cm 

Left Hand Orientation Error 

50,381 

4.86 ± 2.23 

degrees 

Servo Frequency 

67,225 

1005.43 ± 15.68 

Hz 

Servo Compute Latency 

64,118 

0.487 ± 0.0335 

ms 


Table 2. Average statistics of the performance data from one execution of the product disassembly 
task using the 22-DOF Dreamer model. The average range is the standard deviation of the data 
set. The results indicate that average Cartesian position error of the end effectors are about 2-3cm 
and average orientation is about 3-5 degrees. The servo frequency is slightly above the desired 
IkHz and there is jitter despite running within an RTAI real-time context. The servo compute 
latency indicates that on average it only takes about 0.5ms to perform all computations in one 
cycle of the servo loop, which is significantly faster than the 5ms required by UTA-WBC. 


values are the average over 1000 executions of the servo loop. The vast majority 
of the servo loop’s computational latency is from executing the WBOSC algorithm 
to get the next command. Multi-threading significantly decreases the latency of 
updating the model and slightly decreases the latency of computing the command. 
The slightly higher average total latency in the multi-threaded case in Table 
relative to the servo computational latency in Table is most likely due to the 
additional instrumentation that was addded to the servo loop to obtain the detailed 
latency breakdown information. 

The results in Table also show Cartesian positioning errors of up to 5cm 
and orientation errors of up to 30 degrees, though the errors are much less on 
average. Note that the Cartesian position and orientation errors are both model- 
based meaning they are derived from the joint states and the robot model and not 
from external sensors like a motion capture system. Thus, the accuracy of these error 
values depend on the accuracy of the robot’s model and should not be considered 
absolute. However, they do represent the errors that the whole body controller sees 
and attempts to eliminate but cannot because the feedback gains cannot be made 
sufficiently high to remove the errors. 


Figures [l4|c) and 14 'f) indicate a problem with achieving real-time semantics 
on the control PC since the servo frequency and computational latency occasionally 
suffers excessively low and high spikes. The lowest servo frequency measured in this 
sample execution is only 195.3Hz, the maximum is 2.254kHz, and the average is 
1.01 ± 0.016kHz. Coincident with the large spikes in the servo frequency are large 
spikes in the servo compute latency. This indicates that something in the operating 
system or underlying hardware occasionally prevented Controllt !’s real-time servo 
thread from executing as expected. Despite the violations in real-time semantics 
and errors in Cartesian position and orientation, the Controllt! is still able to make 
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Step in Servo Loop 

Multi-Threaded Latency 

Single-Threaded Latency 

Read Joint State 

0.020 ± 0.0020 

0.020 ± 0.0026 

Publish Odometry 

0.014 ± 0.0041 

0.0147 ± 0.00526 

Update Model 

0.0075 ± 0.00256 

0.272 ± 0.00235 

Compute Command 

0.470 ± 0.0128 

0.497 ± 0.0120 

Emit Events 

0.0036 ± 0.00028 

0.0041 ± 0.00027 

Write 

0.0116 ± 0.00075 

0.0125 ± 0.00119 

Total 

0.528 ± 0.0144 

0.820 ± 0.0145 


Table 3. A breakdown of the latencies incurred within one cycle of the servo loop for both the 
single and multi-threaded scenarios using a 22 DOF robot model. All values are in milliseconds 
and are the average and standard deviation over one thousand samples. Most of the latency is 
spent computing the command, which includes executing the WBOSC algorithm. The benefits of 
multi-threading are apparent in the latency of updating the model. 
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Fig. 15. Histograms of the servo frequency and computational latency measured during one exe¬ 
cution of the product disassembly application. The vast majority of the measurements were at the 
desired IKHz frequency and expected 0.5ms computational latency. 


Dreamer reliably perform the task. This is probably because the spikes are rare as 
shown by the histograms of the same data as shown in Figure [Tsl 

5.2. Latency Benchmarks 

The results in Table [^indicate that the servo loop spends about 0.487 ± 0.0335 ms 
computing the next command. This is for a specific compound task with two priority 
levels and 2D orientation tasks and with multi-threading enabled. We now vary the 
compound task configuration in terms of both number of priority levels (which 
affects the number of tasks per priority level) and types of orientation task used. 
We also evaluate both multi-threaded and single-threaded execution of Controllt!. 
All tests involve five tasks: a Cartesian position task for each of the two end 
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Priority Levels / Task 

Allocation 

Orientation 

Task 

Thread¬ 

ing 

Latency (ms) 

2 priority levels 

2D 

multi 

0.528 ± 0.0144 

4 tasks at higher priority 


single 

0.820 ± 0.0145 

1 task at lower priority 

3D 

multi 

0.999 ± 0.0261 



single 

1.289 ± 0.0218 

3 priority levels 

2D 

multi 

0.494 ± 0.0161 

2 tasks at highest priority 


single 

0.764 ± 0.0217 

2 tasks at middle priority 

3D 

multi 

0.788 ± 0.0212 

1 task at lowest priority 


single 

1.068 ± 0.0207 

5 priority levels 

2D 

multi 

0.477 ± 0.0155 

1 task at each level 


single 

0.744 ± 0.0386 


3D 

multi 

0.603 ± 0.0166 



single 

0.882 ± 0.0168 


Table 4. The servo loop’s computational latency when configured with several different compound 
tasks and running in both multi-threaded and single-threaded mode using a 22-DOF model. All 
latencies are the average over 1000 consecutive measurements and the intervals are the standard 
deviations. The results show that the servo loop’s computational latency can be significantly 
decreased using by using multi-threading and placing fewer tasks at each priority level. 


effectors, an orientation task for each of the two end effectors, and a posture task. 
Two types of orientation tasks are used: 2D and 3D. When 2D orientation tasks 
are used, only 5 DOFs of each end effector are controlled by the orientation and 
position tasks; the sixth DOF is controlled by a lower priority posture task. When 
3D orientation tasks are used, all 6 DOFs of each end effector are controlled by the 
orientation and position tasks. 

Three configurations of the compound task are evaluated. The first configuration 
uses two priority levels and assigns all four Cartesian position and orientation tasks 
to be at the higher priority level. The posture task is located at the lower priority 
level. The second configuration uses three priority levels and assigns the Cartesian 
position tasks to be at the highest priority level and the orientation tasks to be 
in the middle priority level. This is possible since the orientation tasks operate 
within the nullspace of the Cartesian position tasks. Like the first configuration, 
the posture task is located at the lowest priority level. The third configuration uses 
5 priority levels. The two Cartesian position tasks are placed in the top two priority 
levels. The two orientation tasks are placed in the next two priority levels. Finally, 
the posture task is located in the lowest priority level. 

The results are shown in Table The use of multi-threading significantly de¬ 
creases computational latency by about 0.2-0.3 ms. Interestingly, distributing the 
tasks across more priority levels decreases computational latency. In this case, plac- 
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ing the orientation tasks and Cartesian position tasks at different priority levels 
results in a significant decrease in servo computational latency. This is because the 
Jacobians and commands of all tasks within the same priority level are concate¬ 
nated into a large matrix and, in this case, performing operations on large matrices 
takes more time than performing multiple operations and nullspace projections us¬ 
ing smaller matrices. 

Note that Controllt! can maintain a IkHz servo frequency in many of the com¬ 
pound task configurations even when running in single-threaded mode. Specifically, 
when 2D orientation tasks are used, IkHz servo frequencies are achieved in all 
compound task configurations. When 3D orientation tasks are used, IkHz servo fre¬ 
quencies can be achieved when the five tasks are spread across five priority levels. 
The 0.882 ± 0.0168 ms that’s achieved in this case is similar to the 0.9 ± 0.045 ms 
that’s achieved using an optimized quadratic programming WBC algorithmic. 


5.3. Flexible End Effector Repositioning 

As previously mentioned, the product disassembly application operates open- loop 
and requires the product to be placed at approximately the same location at the be¬ 
ginning of each execution of the application. For the application to be more robust, 
additional sensors need to be integrated that can determine the actual location of 
the product and communicate this information to the application. Such a sensor 
could be easily integrated since the application is a ROS node meaning it can sim¬ 
ply subscribe to the ROS topic onto which the sensor publishes the actual location 
of the product. Once the application knows where the product is located, it can 
generate the Cartesian space trajectories to allow the end effectors to disassemble 
the product. 

To demonstrate the ability for Controllt! to make Dreamer follow different Carte¬ 
sian space trajectories based on a sensed Cartesian goal coordinate, we created an 
application that makes Dreamer’s right hand move to random Cartesian positions 
while keeping the lower priority joint position task unchanged. The results are 
shown in Figure Note that the right hand is able to move into a wide range 
of Cartesian positions and that the whole body of the robot moves to help achieve 
the goal of the right hand’s Cartesian position task. The elevated error values that 
periodically appear in Figures FlexibleCartesianPositioning (c) and (d) are due to 
the goal Cartesian position being moved beyond the robot’s workspace. Note that 
despite this problem the controller remains stable. This demonstrates Controllt’s 
ability to be integrated into different applications and WBOSC’s ability to handle 
robot redundancies in a predictable and reliable manner. 


6. Discussion 

In this section, we provide a brief history of Controllt’s development followed by 
future research directions. 
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Fig. 16. This figure shows two different perspectives of the same execution of Dreamer changing 
the Cartesian position of her right hand while keeping the lower priority joint position task un¬ 
changed. It demonstrates WBOSC’s ability to handle changes in the goal Cartesian position while 
predictably handling robot redundancies. The error plots show periodically elevated errors when 
the goal Cartesian position is moved beyond the robot’s workspace. The errors are square-shaped 
because of a 5-second pause inserted between successive Cartesian trajectories. The controller 
remains stable despite this problem. 


6.1. History of Controlltl’s Development 

Prior to integration with Dreamer, Controllt! was initially developed for NASA 
JSC’s Valkyrie humanoid robot (now called R5) Software and hardware devel¬ 
opment commenced simultaneously in October 2012. Since hardware development 
took nearly a year, the first year of developing and testing Controllt! involved using 
a simulated version of Valkyrie in Gazebo During this phase, Controllt! was 
initially used to control individual parts of the robot, e.g., each individual limb, 
the lower body, the upper body, and finally the whole robot. By the summer of 
2013, Controllt! was used to control 32-DOFs of Valkyrie in simulation (6 DOFs 
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per leg, 7 DOFs per arm, 3 DOFs in the waist, and 3 DOFs in the neck). Compound 
tasks consisting of up to 15 tasks were employed. They include Cartesian position 
and orientation tasks for the wrists, feet, and the head, an orientation task for the 
chest, a center of mass task and posture task for the whole robot, and center of pres¬ 
sure tasks for the feet. Contact constraints for the hands and feet were configured, 
though not always enabled, depending on whether contact with the environment 
was being made. Management of all of these tasks and constraints were done using a 
higher-level application called Robot Task Commander (RTC) 1^, which provided a 
graphical user interface for operators to instantiate and conhgure controllers based 
on Confront!, integrate these controllers with planners and other processes via ROS 
topics (locomotion was done using a phase space planner and sequence their 
execution within a finite state machine. Integration of Controllt! with Valkyrie in 
simulation was successful. We were able to do most of the DRC tasks including 
valve turning, door opening, power tool manipulation, ladder and stair climbing, 
water hose manipulation, and vehicle ingress. This enabled us to pass the DRC 
critical design review in June 2013 and continue to participate in the DRC Trials 
as a Track A team. 

By the end of Summer 2013, Valkyrie’s hardware development was nearing com¬ 
pletion. At this point we began integrating Controllt! with actual Valkyrie hardware. 
After using Controllt! to control parts of the robots individually, we attempted to 
control all 32 DOFs but ran into problems where gains could not be increased high 
enough to sufficiently reduce errors due to modeling inaccuracies. The robot could 
stand under joint position control but it was not sufficiently stiff to locomote and 
certain joints like the knees and ankles would frequently overheat. We later hypoth¬ 
esized that one problem was likely due to the communication latencies between 
Controllt! and the joint-level controllers being too high. We have since developed a 
strategy called embedded damping to help maintain stability despite the high com¬ 
munication latency Since we could not control all 32 DOFs in time for the DRC 
Trials in December 2013, we resorted to use Controllt! on Valkyrie’s upper body to 
perform several DARPA Robotics Challenge tasks including opening a door, using 
a power tool, manipulating a hose, and turning a valve. Laboratory tests of Con¬ 
trollt! being used to make Valkyrie turn a valve and integrated with the RTC-based 
operator interface is shown in Figure [T?! 

It is important to note that the currently-demonstrable capabilities of WBOSC 
on real hardware is a subset of the capabilities we’ve been able to achieve in sim¬ 
ulation. For example, while preparing for the DRC critical design review in June 
2013, we were able to use Controllt! to make a simulated Valkyrie walk using a 
phase-space locomotion planner and a compound task that controls the center of 
pressures of the feet, the center of mass location, and the internal tensions be¬ 
tween the feet. We will continue to strive to demonstrate these capabilities using 
Controllt! on real hardware. Recent results showing an application-specific imple¬ 
mentation of WBOSC controlling Hume, a point-foot biped, and making it walk in 
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Fig. 17. This figure shows Valkyrie’s upper body being controlled by an early version of Controllt!. 
Using a compound task consisting of Cartesian position and orientation tasks for each hand, and 
a fiat contact constraint for the torso, a human operator uses Valkyrie to turn an industrial valve. 
Parameter binding is used to integrate Controllt! with the operator’s command and visualization 
applications. 


two dimensions is promising 

6.2. Future Research Directions 

As an open-source framework that supports whole body controllers, we hope that 
Controllt! will be adopted by the research community and serve as a common 
platform for developing, testing, and comparing whole body controllers. As a stan¬ 
dalone system that works in both simulation and on real hardware, Controllt! opens 
numerous avenues of research. For example, Controllt! currently allows tasks and 
constraints to be enabled and disabled and to change priority levels at run-time. We 
tested this on hardware by using a joint position task to get the robot into a ready 
state and then switching on higher priority Cartesian position and orientation tasks 
to perform a manipulation application. The transition resulted in a discontinuity 
in the torque signal going to the robot, which is not a problem for an upper body 
manipulation task, but will likely be a problem for legged locomotion. 

We are currently considering two ways to enable smooth WBOSC configuration 
changes. The first method is to gradually introduce the effects of a new task configu¬ 
ration. In this option, the task acceleration or force command is gradually increased 
to reach its actual value. The second method consists of projecting the difference 
between a current compound task’s torque command and the next one in task 
space and adjusting for the difference in a feed-forward manner. This feed-forward 
adjustment can be gradually eliminated to ensure smooth transition between tasks. 
We recently used this technique on Hume, a biped robot, to smoothly transition 
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between contact and non-contact states of the feet 

While Controllt! is designed to support multiple WBC algorithms via plug¬ 
ins, we currently only have two WBC plugins and both are based on WBOSC. 
Other successful WBC algorithms incorporate quadratic programming p5|57|6i|im| 
Unlike WBOSC that analytically solves the WBC problem, quadratic program¬ 
ming is an optimization method that more naturally supports inequality con¬ 
straints. While quadratic programming is computationally intensive, recent progress 
on methods to simplify quadratic programming-based whole body controllers have 
enabled them to execute in less than 1ms on robots with two fewer joints than 
Dreamer 1^. As future work, it would be interesting to determine (1) whether 
quadratic programming-based whole body controllers could be implemented as a 
plugin within Controlltl’s architecture and (2) the pros and cons of WBOSC rel¬ 
ative to quadratic programming-based whole body controllers. Note that others 
have developed formulations similar to WBOSC that include support for inequal¬ 
ity constraints and solve them using quadratic programming^. The integration of 
on-line optimization techniques to allow the incorporation of inequality constraints 
is an area of future work and may require modifying the current constraint API to 
include a specification of whether the constraint is negative or positive. 

To the best of our knowledge, there are no other multi-threaded open source 
implementations of WBOSC or other forms of whole body controllers. We are cur¬ 
rently unable to prove that our multi-threaded design consisting of a real-time servo 
thread with two child threads is optimal. Other choices certainly exist. For example, 
the two child threads could be combined into a single child thread that updates both 
the model and the tasks. Going in the opposite direction, a separate child thread 
could be instantiated for each task where there is one thread per task. Performing a 
more detailed analysis on the ideal multi-threaded architecture is a future research 
direction. 

One consequence of adopting a multi-threaded strategy is the robot model is 
no longer updated synchronously with the servo thread and thus can become stale. 
We currently do not use any metric to determine when the model has become 
excessively stale. A child thread simply updates the model as quickly as possible. 
For our product disassembly task, the child thread was able to update the model 
fast enough to enable WBOSC to reliably complete the task. An interesting research 
direction is to formally investigate how stale a model can be before it negatively 
impacts robot performance. The answer will likely depend on the robot’s current 
configuration. 

A given constraint can have an infinite number of null space projectors. The 
one we use in Controllt! is the Dynamically Consistent Null Space Projector 
The nullspace projector is currently derived within the constraint set. Given the 
existence of alternative null space projectors, a potential improvement to Controllt! 
would be to make the constraint set extensible via plugins. The default plugin will 
use the current Dynamically Consistent Null Space Projector. However, the user 
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can easily override this by providing a constraint set plugin that provides another 
null space projector. 

The results in Section |5.1| show that the control PC occasionally has latency 
spikes that violate the desired servo frequency. Learning why the latency spikes 
occur is useful since eliminating them will likely increase system reliability or at 
least predictability. However, we have yet to notice the latency spikes causing any 
problem during our extensive use of Dreamer. It’s worth noting that Dreamer is a 
COTS robot and its control PC was configured by the robot’s manufacturer. Given 
that the control PC was pre-configured for us, from our perspective, it is somewhat 
of a “black box”. If the need arises (i.e., the latency spikes actually prevent us from 
executing a particular task), we will investigate the latency spikes using a two¬ 
pronged approach. First, we will instrument the Linux kernel with debug messages 
that help track down when the latency spikes occur. Second, we will remove all 
unnecessary kernel modules and disable all unnecessary hardware until the latency 
spikes no longer occur. We will then slowly add hardware and software modules 
re-testing for latency spikes after each addition. Once the latency spikes return, we 
know which hardware or software module caused it and can investigate it further. 

In this paper, we did not explicitly account for singularities but they did not 
pose a problem in our tests even when the arms are fully stretched out as described 
in Section 5.3. This is probably due to our choice of the tolerances for computing 
pseudo-inverses within the controller. However, we have not performed a detailed 
study on adequate tolerances nor on handling singularity thus far. 

Other future research areas include how to add adaptive control capabilities that 
continuously improve the robot model based on observed robot behavior, which 
should enable the resulting WBOSC commands to have an increasingly high feed¬ 
forward component and lower feedback component, and the integration of Controllt! 
with external sensors to enable, for example, visual servoing. 


7. Conclusions 

With the increasing availability of sophisticated multi-branched highly-redundant 
robots targeted for general applications, whole body controllers will likely become 
an essential component in advanced human-centered robotics. Controllt! is an open- 
source software framework that defines a software architecture and set of APIs for 
instantiating and configuring whole body controllers, integrating them into larger 
systems and different robot platforms, and enabling high performance via multi¬ 
threading. While it is currently focused on facilitating the integration of controllers 
based on WBOSC, the software architecture is highly extensible to support addi¬ 
tional WBC algorithms and control primitives. 

This paper provided a software framework that enables the quick instantiation 
and configuration of WBOSC behaviors for practical applications such as a product 
disassembly task using a 22-DOF humanoid upperbody robot. The experiments 
demonstrated high performance with servo computational latencies of about 0.5ms. 
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In summary, WBC is a rich and vibrant though fragmented research area today 
with numerous algorithms and implementations that are not cross-compatible and 
thus difficult to compare in hardware. We present Controllt! as a software frame¬ 
work for supporting the development and study of whole body operational space 
controllers and their integration into useful advanced robotic applications. 
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Appendix A. Controllt! Dependencies 


Dependency 

Ver¬ 

sion 

Purpose 

g++ 

4.8.2 

or 

4.6.3 

Compiler for C-|—hll programming language 

Eigen 

3.0.5 

Linear algebra operations 

RBDL 

2.3.2 

Robot modeling, forward and inverse kinematics and 
dynamics 

URDF 

1.11.6 

Parsing robot model descriptions 

ROS 

Hy¬ 

dro 

or In¬ 
digo 

Component-based software architecture, useful 
libraries like pluginlib, runtime support like a 
parameter server and roslaunch bootstrapping 
capabilities 

RTAI 

3.9 

Real-time execution semantics (only required when 
using Dreamer or other RTAI-compatible robot) 

Gazebo 

5.1.0 

Test controller in simulation prior to on real hardware 


Table 5. Controllt! dependencies. 
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Appendix B. Controllt! Parameters 

Tables [6][^ contains additional Controllt! parameters that can be loaded onto the 
ROS parameter server. They must be namespaced by the controller’s name. 


Name 

Description 

coupled-joint-groups 

Specifies which groups of joints should be coupled. 
Effectively modifies the model to decouple group of 
joints from each other. This is useful for debugging 
purposes or to account for modeling inaccuracies. It is 
an array of array of strings. 

enforce_effort -limits 

Whether to enforce joint effort limits. These limits are 
specified in the robot description. If true, effort 
commands exceeding the limits will be truncated at the 
limit and a warning message will be produced. It is an 
array of Boolean values. 

enforce-positiou-limits 

Whether to enforce joint position limits. These limits 
are specified in the robot description. If true, position 
commands exceeding the limits will be truncated at the 
limit and a warning message will be produced. It is an 
array of Boolean values. 

enforce-velocityJimits 

Whether to enforce joint velocity limits. These limits 
are specified in the robot description. If true, velocity 
commands exceeding the limits will be truncated at the 
limit and a warning message will be produced. It is an 
array of Boolean values. 

gravity-compensatiou-mask 

Specifies which joints should not be gravity 
compensated. This is useful when certain joints have so 
much friction that gravity compensation is not 
necessary. It is an array of joint name strings. 

log-level 

The log level, which can be DEBUG, INFO, WARN, 
ERROR, or FATAL. This controls how much log 
information is generated during run-time. It is a string 
value. 


Table 6. Controllt! parameters (1 of 2). 
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Name 

Description 

log_fields 

Specifies the optional fields that are in a log message’s 

prefix. Possible values include: 

package - the ROS package containing the message 

file - file containing the message 

line - the line number of the message. 

function - the method producing the message 

pid - the process ID of the thread producing the 

message 

It is an arry of strings. 

max_effort_command 

Specifies the maximum effort that should be 
commanded for each joint. A warning is produced if 
this is violated. It is an arry of integers. 

parameter_binding_factories 

The names of the plugins containing the parameter 
binding factories to use. It is an array of strings. 

robot_description 

Contains the URDF description of the robot. This is 
used to initialize Controllt’s floating model. It is a 
string value. 

robot_interface_type 

The name of the robot interface plugin to use. It is a 
string. 

servo _clock_type 

The name of the servo clock plugin to use. It is a string 
value. 

servo _frequency 

The desired servo loop frequency in Hz. Warnings will 
be published if this frequency is not achieved. It is an 
integer value. 

single_threaded_model 

Whether to use the servo thread to update the model. 

It is a Boolean value. 

single_threaded_tasks 

Whether to use the servo thread to update the task 
states. It is a Boolean value. 

whole_body .controller _type 

The name of the WBC plugin to use. It is a string 
value. 

world_gravity 

Specifies the gravity acceleration along the X, Y, and Z 
axis of the world frame. Defaults to (0, 0, —9.81). This 
is useful for debugging or when working in worlds 
where the gravity does not pull in negative Z axis 
direction. It is an integer array. 


Table 7. Controllt! parameters (2 of 2). 
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Appendix C. Controllt! Introspection Capabilities 

This appendix describes Controllt !’s introspection capabilities, which enable users 
to gain insight into the internal states of the controller. 

Task-based introspection capabilities. Tasks can configured to publish ROS 
visualization_msgs/MarkerArray 

and visualizationjnsgs/InteractiveMarkerUpdate messages onto ROS topics 
that show the current and goal states of the controller. These messages can be visu¬ 
alized in RViz to understand what the task-level controller is trying to achieve. For 
example, Figure shows the marker array messages published by a 2D orientation 
task. The green arrow shows the goal heading whereas the blue arrow shows the 
current heading. Figure[^shows visualizations of 2D and 3D orientation tasks and 
Cartesian position tasks. 

Figure shows visualizations of the actual and desired center of pressures and 
the current center of mass projected onto the ground. This information is useful to 
visually determine the stability of the current posture. 

ROS service-based introspection capabilities. Table lists the various 
service-based controller introspection capabilities that are provided by Controllt!. 
These services can be called by external processes and are useful for integrating 
Controllt! into a larger system. All services are namespaced by the controller’s 
name enabling multiple instances of Controllt! to simultaneously exist. 

ROS topic-based introspection capabilities. Tablej^lists the various topic- 



Fig. 18. When integrated with Trikey, Controllt! can be configured to publish ROS 
visualizationjnsgs/MarkerArray messages containing the current and goal headings of the robot. 
These marker messages can be visualized in RViz. The green arrow is the goal heading, whereas 
the blue arrow is the current heading. In this screenshot, Controllt! is in the process of rotating 
Trikey counter clockwise when viewed from above. 
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(a) 



(b) 


Fig. 19. Two Cartesian position tasks and two orientation tasks are used to position and orient 
Dreamer’s end effectors in the world. The orientation and Cartesian position tasks are higher 
priority than a joint position task that defines the robot’s posture, (a) Shows the current and goal 
2DOF orentations. (b) Shows how ROS 6-DOF interactive markers denote the current position 
and orientation of the wrists. The interactive markers can be dynamically and visually changed 
by the user to update the goal positions and orientaions of the robot’s wrists. 


based controller introspection capabilities that are provided by Controllt!. These 
topics can be subscribed to by external processes and are useful for integrating 
Controllt! into a larger system. All topics are namespaced by the controller’s name 
enabling multiple instances of Controllt! to simultaneously exist. 
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Fig. 20. This is a screenshot from a Gazebo simulation where Controllt! was used to make an early 
prototype of Valkyrie walk six steps. Controllt’s compound task consisted of a Center-Of-Mass 
(COM) task, posture task, Cartesian position task for the hip height, prototype internal tensions 
task, and, for each foot, a Cartesian position task, orientation task, and Center- of-Pressure (COP) 
task. The red balls mark the goal COP locations of the feet, yellow balls are the current COP 
locations, and the blue ball is the COM projected onto the ground. Note that in this screenshot 
the yellow balls, which represent the actual COPs, are on the left edge of the feet, meaning the 
feet are very close to rolling clockwise when viewed from the front. 
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Service 

Description 

diagnostics/getActuableJointIndices 

Provides the order of every 
actuable joint in the robot model 
(omits joints that are real but 
not actuable) 

diagnostics/getCmd Jointindices 

Provides the order of the joints in 
the command issued by 

Controllt! to the robot. 

diagnostics/getConstraintJacobianMatrices 

Provides the Jacobian matrices 
belonging to the constraints in 
the constraint set. 

diagnostics / getConstraintParameters 

Provides a list of every constraint 
parameter and its current value. 

diagnostics/getControlItParameters 

Provides the current values of the 
Controllt! parameters defined in 
Appendix A.2. 

diagnostics / getControllerConfiguration 

Provides the current state of the 
compound task and constraint 
set. 

diagnostics/getRealJointIndices 

Provides the order of every real 
joint in the robot model. 

diagnostics/getTaskParameters 

Provides a list of every task 
parameter is its current value. 


Table 8. ControlItPs ROS service-based controller introspection capabilities. 




June 4, 2015 0:19 


mam 


46 C.-L. Fok, G. Johnson, J. D. Yamokoski, A. Mok, and L. Sentis 


Service 

Description 

diagnostics/RTTCommLatency 

Publishes the latest round-trip 
communication time between Controllt! 
and the joint-level controllers. This is 
done by transmitting sequence numbers to 
the joint-level controllers, which are 
reflected back through the joint state 
data. Controllt! monitors the time 
between transmitting a particular 
sequence number and receiving it back. 

diagnostics/command 

Publishes the latest command issued by 
Controllt! to the robot. 

diagnostics/errors 

Publishes any run-time errors that are 
encountered. An example error is when 
the command includes NaN values. 

diagnostics / gravity Vector 

Publishes the current gravity 
compensation vector. 

diagnostics/jointState 

Publishes the latest joint state 
information. 

diagnostics / modelLatency 

Publishes the staleness of the currently 
active model. The model latency is the 
time since the model was last updated. 

diagnostics/servoComputeLatency 

Publishes the amount of time it took to 
execute the computations within one cycle 
of the servo loop. 

diagnostics/servoFrequency 

Publishes the instantaneous servo 
frequency. 

diagnostics/warnings 

Publishes any run-time warnings that are 
encountered. An example warning is when 
the joint position or velocity exceeds 
expected limits. 


Table 9. Controlltl’s ROS topic-based controller introspection capabilities. 






June 4, 2015 0:19 


mam 


Controllt! - A Software Framework for Whole-Body Operational Space Control 47 

Appendix D. Controllt! Configuration File 

Controllt! enables user to specify the controller configuration using a YAML con¬ 
figuration file. The syntax of this file is shown below. By enabling YAML-based 
configuration, Controllt! can be made to work with a wide variety of applications 
without modifying the source code and recompiling. 

Task specification: 
tasks: 

- name: [task name] # user defined 

type: [task type] # must match plugin name 
... # task-specific parameters and their values 
... # additional tasks 

Constraint specification: 

constraints: 

- name: [constraint name] # user defined 

type: [constraint type] # must match plugin name 
... # constraint-specific paremieters and their values 
... # additional constraints 

Compound task specification: 

compound_task: 

- name: [task name] 
priority: [priority level] 
operational_state: [enable or disable] 

... # additional tasks 

Constraint set specification: 

constraint_set; 

- name: [constraint name] 
type: [constraint type] 
operational_state: [enable or disable] 

... # additional constraints 

Binding Specification: 
bindings: 

- parameter: [paramieter name] # must match real paremieter name 
direction: [input or output] 

topic; [topic narnie] 

transport_type: [transport type] # must match plugin narnie 
properties; 

- [transport-specific property] 
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... # additional transport-specific properties 
... # additional bindings 

Event Specification: 

events: 

- namie: [event name] # user defined 

expression: [logical expression over paramieters] 
... # additional events 



June 4, 2015 0:19 


mam 


Controllt! - A Software Framework for Whole-Body Operational Space Control 49 


Appendix E. Controller Gains 

The following tables provide the gains used by the various controllers in the product 
disassembly application using Dreamer. The negative joint position controller gains 
are strange but were configured as such by Meka Robotics, the robot’s manufacturer 
(Meka Robotics has since been bought by Google). We don’t know for sure why 
some gains are negative since we are unable to access the details of the joint-level 
controllers. It’s possible that the direction of the encoder is opposite of the motor 
resulting in the need for negative gains. Regardless, these were the functioning 
settings used in the development and testing of Controllt! on Dreamer. 

The reason why the left and right arms have different gains is because the left 
arm is about three years newer than the right arm and internally the mechatronics 
of the left arm are significantly different from that of the right arm. 


Controller 

Kp 

Ki 

Kd 

torso _lower_pitch 

-3 

0 

0 

left .shoulder .extensor 

10 

1 

0 

left .shoulder .abductor 

10 

1 

0 

left .shoulder .rotator 

10 

1 

0 

left .elbow 

10 

1 

0 

left.wrist.rotator 

50 

0 

0 

left.wrist.pitch 

15 

0 

1 

left.wrist.yaw 

15 

0 

1 

right.shoulder .extensor 

7 

0 

0 

right.shoulder .abductor 

6 

0 

0 

right.shoulder .rotator 

5 

0 

0 

right.elbow 

5 

0 

0 

right.wrist .rotator 

-3 

0 

1 

right .wrist .pitch 

-15 

0 

-1 

right .wrist .yaw 

-15 

0 

-1 


Table 10. Dreamer joint torque controller gains. 
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Task 

Kp 

Ki 

Kd 

Joint Position Task 

60 

0 

3 

Left Hand Orientation 

60 

0 

3 

Right Hand Orientation 

60 

0 

3 

Left Hand Position 

64 

0 

3 

Right Hand Position 

64 

0 

3 


Table 11. Controllt! Task-level controller gains used to control Dreamer. 
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Appendix F. Example Application Code 

Figure contains an example code fragment from the product disassembly . The 
application is written in the Python programming language, though any program¬ 
ming language supported by ROS could be used including C-|—b. The code fragment 
shows how the Cartesian position trajectory is generated for moving the right hand 
into a position where it can grab the metal tube. Lines 548-552 specify the Carte¬ 
sian (x, y, z) waypoints that the hand is expected to traverse. For brevity, only 
one waypoint is shown. Line 555 creates a cubic-spline interpolator, which is used 
on line 559 to generate the intermediate points between the waypoints. The while 
loop starting on line 564 obtains the current goal Cartesian position based on the 
elapsed time (line 572) and transmits this goal via a ROS topic (line 576). The goal 
parameter of the right hand Cartesian position task within Controllt! is bound to 
this ROS topic enabling Controllt! to follow the desired Cartesian trajectory. The 
trajectory is transmitted at lOOHz, based on line 579. Once the trajectory is done, 
line 583 issues a command to close the fingers in the right hand is issued via another 
bound ROS topic. 
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546 def grabMetalTubeCself ) : 

547 

548 # define the right hand Cartesian position trajectory waypoints 

549 rjCartWP = [] 

550 rjCartWP.appendC [0.25822435038901964, -0.1895604971725577, 

551 1.0461857180093073]) 

552 # insert more waypoints here 

553 

554 # create a cubic-spline-based trajectory generator 

555 rhCartTG = TrajectoryGeneratorCubicSpline 

556 . TrajectoryGeneratorCubicSpline (rjCartWP) 

557 

558 # generate the trajectory over a certain time interval 

559 TOTAL_TRAVEL_TIME = 5.0 # seconds 

560 rhCartTG. generateTrajectory(TOTAL_TRAVEL_TIME) 

561 

562 # execute the trajectory 

563 done = False 

564 while not done and not rospy. is_shutdown() : 

565 

560 # compute elapsed time 

567 deltaTime = self. getTimeSeconds () - startTime 

568 if deltaTime >= TQTAL.TRAVEL.TIME: 

569 done = true 

570 

571 # get the current Cartesian position along trajectory 

572 goal = rhCartTG. getPoint (deltaTime) 

573 

574 # save Cartesian goal into ROS message and publish it 

575 self .rhCartGoalMsg.data = goal 

576 self . rhCartTaskGoalPublisher .publishCself .rhCartGoalMsg) 

577 

578 if not done : 

579 rospy . sleep(0.01) # lOOHz 

580 

581 # do 'power grasp 

582 self .rightHandCmdMsg.data = True 

583 self .rightHaindCmdPublisher .publishCself .rightHandCmdMsg) 

584 )■ 


Fig. 21. Code fragment from product disassembly application 
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Appendix G. Controllt! SMACH FSM Integration 

The following screenshot is a visualization of the product disassembly finite state 
machine provided by ROS SMACH Visualizer. It is updated in real-time as the 
application in running. This particular screenshot shows that Dreamer is in the 
“GrabValveState” which is when her left gripper is being positioned to grab the 
valve. 



/SM_ROOT/SleepState:doneSleep;/SM_ROOT/ReinoveValveState 

Fig. 22. This figure shows a visualization of the FSM used by the product disassembly application. 
The ROS package SMACH is used to both implement the FSM logic and visualize its execution. 
The green arrow indicates the current state of the demo. 
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